Self-position estimation method

ABSTRACT

A method capable of appropriately estimating (specifying) a self-position of a mobile object while appropriately correcting an estimated value of a self-position by an SLAM algorithm is provided. In a self-position estimation method, an actual self-position of a mobile object 1 is specified (fixed) from self-positions estimated by a plurality of algorithms. The plurality of algorithms includes an SLAM algorithm (12) and an algorithm (11) different from the SLAM algorithm. A correction processing unit 16 intermittently corrects an estimated value of a self-position obtained by the SLAM algorithm in accordance with any one self-position out of an estimated value of a self-position obtained by an algorithm other than SLAM and a specified self-position.

CROSS REFERENCE TO RELATED APPLICATIONS

This application claims the priority benefit of Japanese PatentApplication No. 2019-057026, filed on Mar. 25, 2019. The entirety of theabove-mentioned patent application is hereby incorporated by referenceherein and made a part of this specification.

BACKGROUND Technical Field

The present disclosure relates to a method of estimating a self-positionof a mobile object.

Description of Related Art

Hitherto, various techniques for estimating a self-position of a mobileobject such as a mobile robot using detected information of a pluralityof sensors have been proposed. For example, in Patent Document 1(Japanese Patent Application Laid-Open No. 2007-149088), a technique forestimating a self-position of a mobile object using detected informationof an internal sensor such as an encoder or a gyroscope and detectedinformation of an external sensor such as a distance measurement sensor,an IR sensor, an ultrasonic sensor, or a camera has been proposed.

In addition, various algorithms including an algorithm for estimating aself-position of a mobile object using detected information of one ormore sensors have been hitherto proposed.

Incidentally, in a case where self-positions are estimated using aplurality of algorithms from detected information of a plurality ofsensors and a self-position of a mobile object is specified from anestimated value of the plurality of self-positions, a simultaneouslocalization and mapping (SLAM) algorithm may be adopted as onealgorithm among the plurality of algorithms.

However, the SLAM algorithm is configured to estimate a moved positionof a mobile object with respect to an initial position by sequentiallycalculating movement amounts (position variations) of the mobile objectin a local coordinate system based on the initial position of the mobileobject and multiplying the movement amounts. For this reason, in a casewhere a self-position of a mobile object in a global coordinate systemwhich is arbitrarily set with respect to a movement environment of themobile object is estimated, an error of the self-position of the mobileobject obtained by an SLAM algorithm may increase in an integrationmanner due to an error of a set value of the initial position of themobile object seen in the global coordinate system or errors ofsequentially calculated movement amounts. Further, when a self-positionof a mobile object is specified by reflecting the self-position of themobile object which is obtained by a SLAM algorithm, there is a concernthat reliability of the specified self-position may be impaired.

The disclosure provides a method by which a self-position of a mobileobject can be appropriately estimated (specified) while an estimatedvalue of a self-position is appropriately corrected using a SLAMalgorithm in a method of estimating a self-position of a mobile objectusing a plurality of algorithms including a SLAM algorithm.

SUMMARY

According to one embodiment of the disclosure, there is provided aself-position estimation method of estimating a self-position of amobile object by each of a plurality of algorithms using detectedinformation of one or more sensors among a plurality of sensors fromdetected information of the plurality of sensors and specifying aself-position of the mobile object from estimated values of theself-positions obtained by the plurality of algorithms. The plurality ofalgorithms comprises an first algorithm which is a simultaneouslocalization and mapping (SLAM) algorithm and a second algorithm whichis an algorithm different from the SLAM, and the self-positionestimation method comprises a correction step of intermittentlycorrecting an first self-position estimated value which is an estimatedvalue of the self-position obtained by the first algorithm in accordancewith any one self-position out of a second self-position estimated valuewhich is an estimated value of the self-position obtained by the secondalgorithm and a self-position specification value which is the specifiedself-position.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a diagram showing a schematic configuration of a mobile objectin an embodiment of the disclosure.

FIG. 2 is a block diagram showing processing performed by aself-position estimation processing apparatus shown in FIG. 1 .

FIGS. 3A to 3C are diagrams showing processing of a correctionprocessing unit shown in FIG. 2 .

DESCRIPTION OF THE EMBODIMENTS

An embodiment of the disclosure will be described below with referenceto FIG. 1 to FIGS. 3A to 3C. Referring to FIG. 1 , a mobile object 1 inthe present embodiment, which is, for example, a wheel type mobileobject, comprises a body 3 which is supported on a floor surface of amovement environment through a plurality of wheels 2. In addition, themobile object 1 moves on the floor surface by rotationally driving oneor more wheels 2 using an actuator such as an electric motor not shownin the drawing or performing rotation driving and steering (directionconversion) of the one or more wheels 2.

A plurality of sensors used to estimate a self-position are mounted inthe mobile object 1. For example, an odometry sensor 5 such as anangular sensor which detects rotation angles of one or more wheels 2, alaser range finder 6 (hereinafter referred to as an LRF 6) as a distancemeasurement sensor which measures a distance to an external object nearthe mobile object 1, and a camera 7 which captures an image of a ceilingabove the mobile object 1 are mounted in the mobile object 1. Meanwhile,the odometry sensor 5 may include a sensor that detects a steering angleof a steering wheel among the wheels 2 in addition to an angular sensorthat detects a rotation angle of the wheel 2. In addition, the camera 7may be a stereo camera in the present embodiment for example.

A self-position estimation processing apparatus 10 that estimates aself-position using detected information of these sensors is mounted inthe mobile object 1. The self-position estimation processing apparatus10 is constituted by one or more electronic circuit units including, forexample, a microcomputer, a memory, an interface circuit, and the like.

In addition, the self-position estimation processing apparatus 10comprises a first self-position estimation processing unit 11, a secondself-position estimation processing unit 12, a weighting factor learningprocessing unit 14, a self-position specification processing unit 15,and a correction processing unit 16 as shown in a block diagram of FIG.2 as functions realized by either one or both of a hardwareconfiguration and a program (software configuration) mounted therein.Meanwhile, the self-position estimation processing apparatus 10 is notlimited to the above-described functions, and may also include, forexample, a function of performing movement control of the mobile object1, and the like.

The first self-position estimation processing unit 11 is a functionalunit that executes a process of estimating a self-position of the mobileobject 1 using detected information of the LRF 6 and detectedinformation of the odometry sensor 5, and the second self-positionestimation processing unit 12 is a functional unit that sequentiallyexecutes processes of estimating a self-position of the mobile object 1using detected information of the camera 7.

Meanwhile, in the present embodiment, in more detail, a self-position ofthe mobile object 1 which is estimated by each of the first and secondself-position estimation processing units 11 and 12 is a set of theposition and posture (orientation) of the mobile object 1 seen in aglobal coordinate system which is arbitrarily set in a movementenvironment of the mobile object 1. The self-position of the mobileobject 1 which is estimated by the first self-position estimationprocessing unit 11 is equivalent to a second self-position estimatedvalue in the disclosure, and the self-position of the mobile object 1which is estimated by the second self-position estimation processingunit 12 is equivalent to a first self-position estimated value in thedisclosure. In the following description, the self-position estimated byeach of the first and second self-position estimation processing units11 and 12 may be referred to as estimated position posture.

In addition, the weighting factor learning processing unit 14 is afunctional unit that outputs a weighting factor corresponding to each ofthe first and second self-position estimation processing units 11 and 12(in detail, a weighting factor for an estimated position posture of themobile object 1 which is output from each of the first and the secondself-position estimation processing units 11 and 12) in accordance withstate quantities A and B to be described later which are input from thefirst and second self-position estimation processing units 11 and 12,and the self-position specification processing unit 15 is a functionalunit that specifies a self-position of the mobile object 1 (determinesan estimated position posture of the mobile object 1) by combiningestimated position postures of the mobile object 1 which are output fromeach of the first and second self-position estimation processing units11 and 12 with each other in accordance with a weighting factor which isoutput from the weighting factor learning processing unit 14. Theself-position of the mobile object 1 which is specified by theself-position specification processing unit 15 is equivalent to aself-position specification value in the disclosure.

In addition, the correction processing unit 16 is a functional unit thatappropriately generates correction parameters for correcting aself-position (estimated position posture) estimated by the secondself-position estimation processing unit 12 and inputs the generatedcorrection parameters to the second self-position estimation processingunit 12.

Hereinafter, the overall processing of the self-position estimationprocessing apparatus 10 will be described together with details of theabove-described functional units.

Processes of the first and second self-position estimation processingunits 11 and 12 are executed as described below. Detected information ofeach of the LRF 6 and the odometry sensor 5 is input to the firstself-position estimation processing unit 11, and map data (for example,a two-dimensional occupied grid map or the like) of a movementenvironment of the mobile object 1 is input thereto. In this case, themap data is stored and held in advance in the memory of theself-position estimation processing apparatus 10 or is downloaded at anytime from an external server or the like.

In addition, the first self-position estimation processing unit 11sequentially estimates self-positions of the mobile object 1(sequentially determines estimated position postures) using apredetermined algorithm for self-position estimation from detectedinformation (distance measurement data of an external object in thevicinity of the mobile object 1) of the LRF 6, detected information ofthe odometry sensor 5, and map data.

In the present embodiment, the first self-position estimation processingunit 11 uses an algorithm for estimating a self-position using aparticulate filter, for example, an algorithm of a known method such asadaptive Monte Carlo localization (AMCL) as an algorithm forself-position estimation. The algorithm is equivalent to a secondalgorithm in the disclosure.

In the AMCL algorithm, self-positions of the mobile object 1(self-positions seen in a global coordinate system) are sequentiallyestimated through the following process. That is, first, a plurality ofparticles in a particulate filter are generated near the mobile object 1in accordance with the initial position and the initial posture(orientation) of the mobile object 1. Meanwhile, the initial positionand the initial posture of the mobile object 1 are specified usingdetection data of the LRF 6 (distance measurement data of an externalobject in the vicinity of the mobile object 1) and a map, for example,at the start of the movement of the mobile object 1 or immediatelybefore that.

In addition, a new moved position of each particle is estimated inaccordance with a moving speed (a translational speed and an angularvelocity) of the mobile object 1 which is estimated from detectedinformation of the odometry sensor 5. Subsequently, distance measurementdata of an external object in the vicinity of the mobile object 1 isacquired using the LRF 6, and the likelihood of a new moved position ofeach particle is calculated on the basis of the distance measurementdata and map data. In addition, particles having a low likelihood areeliminated through a resampling process, and new particles are generatednear particles having a high likelihood.

In the AMCL algorithm, self-positions of the mobile object 1 aresequentially estimated (estimated position postures of the mobile object1 are sequentially estimated) on the basis of a time series of theposition of particles having a high likelihood which are obtained bysequentially executing the above-described processing. However, in thepresent embodiment, a posture among the self-positions (estimatedposition postures) of the mobile object 1 which are estimated using theAMCL algorithm is a posture of the mobile object 1 in a yaw direction (adirection around an axis in a vertical direction), and a posture in aroll direction (a direction around an axis in a front-back direction ofthe mobile object 1) and a posture in a pitch direction (a directionaround an axis in a right-left direction of the mobile object 1) are notincluded.

Further, in the AMCL algorithm, a covariance (in detail, avariance-covariance) as a state quantity related to the certainty ofestimated position posture is generated in a process of executing aself-position estimation process of the mobile object 1. In addition,the first self-position estimation processing unit 11 sequentiallyoutputs the self-position (estimated position posture) of the mobileobject 1 which is estimated by the AMCL algorithm as described above andan estimation time which is the time of the estimation, and outputs thecovariance (variance-covariance) as a state quantity related to thecertainty of the estimated position posture (hereinafter referred to asa state quantity 1).

As a supplement, in a case where an abnormality such as the estimatedposition posture of the mobile object 1, which is determined using anAMCL algorithm, deviating from a predetermined range and a variation inthe estimated position posture (a variation for each estimation processcycle) deviating from a predetermined range occurs, the firstself-position estimation processing unit 11 corrects the estimatedposition posture by the self-position specification processing unit 15in accordance with the self-position of the mobile object 1 which isspecified as will be described later. For example, the estimatedposition posture of the mobile object 1 is corrected to match or comeclose to a self-position which is specified immediately before by theself-position specification processing unit 15.

A captured image (a captured image of the ceiling above the mobileobject 1) is input to the second self-position estimation processingunit 12 as detected information of the camera 7. Meanwhile, in thepresent embodiment, since the camera 7 is a stereo camera, the detectedinformation (captured image) of the camera 7 which is input to thesecond self-position estimation processing unit 12 is a stereo image (apair of captured images). However, the camera 7 is not limited to astereo camera and may be a monocular camera.

In addition, the second self-position estimation processing unit 12sequentially estimates self-positions of the mobile object 1(sequentially determines estimated position postures) using apredetermined algorithm for self-position estimation from the inputcaptured image.

In the present embodiment, the second self-position estimationprocessing unit 12 uses an algorithm for performing detection of afeature point from an input captured image and matching between thefeature point and a feature point detected from a captured image whichwas input in the past, as an algorithm for self-position estimation.More specifically, the second self-position estimation processing unit12 uses an algorithm of a known method such as ORB_SLAM as a method ofVisual_SLAM as the algorithm. The algorithm is equivalent to a firstalgorithm in the disclosure.

In the ORB_SLAM algorithm, self-positions of the mobile object 1 aresequentially estimated through the following process. That is, afterinitial position postures of the mobile object 1 (a set of an initialposition and an initial posture of the mobile object 1 seen in a globalcoordinate system) are set, captured images (stereo images) obtained bythe camera 7 are sequentially input to the second self-positionestimation processing unit 12, and a process of detecting an ORB featurepoint from the captured image for each input captured image issequentially performed. Meanwhile, regarding the initial positionposture of the mobile object 1 in the process of the secondself-position estimation processing unit 12, an estimated positionposture of the mobile object 1 which is obtained by the firstself-position estimation processing unit 11 at the start of the movementor immediately before the start of the movement is set, for example,when the mobile object 1 starts to move. However, the initial positionposture is appropriately reset through a process of the correctionprocessing unit 16 to be described later.

In addition, an ORB feature point detected from a newly input capturedimage (a captured image at time t) and an ORB feature point detectedfrom a past captured image (a captured image at time t−Δt) which isinput one captured image before are compared with each other, andmatching between the ORB feature points is performed. In the matching,it is determined that ORB feature points in which a magnitude (absolutevalue) of a difference in feature amount is equal to or less than apredetermined threshold value are the same ORB feature points (ORBfeature points corresponding to each other). Meanwhile, Δt is a samplingcycle of captured images.

In addition, a movement amount and a moving direction of the mobileobject 1 in a period between time t and time t−Δt are estimated on thebasis of the matching, and an estimated position posture of the mobileobject 1 at time t is determined from estimated values of the movementamount and the moving direction and an estimated position posture of themobile object 1 at the time t−Δt.

In more detail, the movement amount and the moving direction of themobile object 1 in a period between two time t and time t−Δt areestimated as a movement amount and a moving direction seen in a localcoordinate system for a self-position estimation process (a localcoordinate system with an initial position as a starting point) using anORB_SLAM algorithm. In addition, an estimated position posture (anestimated position posture in a global coordinate system) of the mobileobject 1 at time t is obtained by performing coordinate transformationof estimated values of the movement amount and the moving direction ofthe mobile object 1 in a period between time t and time t−Δt to amovement amount and a moving direction seen in a global coordinatesystem and adding the movement amount and the moving direction havingbeen subjected to the coordinate transformation to estimated positionpostures (estimated position postures in a global coordinate system) ofthe mobile object 1 at time t−Δt. In this case, the coordinatetransformation is determined in accordance with a relative position andposture relationship between the local coordinate system and the globalcoordinate system which is specified on the basis of the initialposition posture of the mobile object 1 seen in a global coordinatesystem.

In the ORB_SLAM algorithm, basically, self-positions of the mobileobject 1 are sequentially estimated (estimated position postures of themobile object 1 are sequentially determined) by sequentially executingthe above-described process. In this case, postures among the estimatedposition postures obtained by the ORB_SLAM algorithm include not only aposture in a yaw direction but also postures in a roll direction and apitch direction.

Meanwhile, in the present embodiment, in a case where correctionparameters determined by the correction processing unit 16 as will bedescribed below are imparted from the correction processing unit 16, thesecond self-position estimation processing unit 12 corrects aself-position (estimated position posture) estimated as described abovein accordance with the correction parameters. This correction processwill be described later.

Further, in the ORB_SLAM algorithm, an estimated speed and an angularvelocity (estimated values of a translational speed and an angularvelocity) of the mobile object 1, the number of detected ORB featurepoints (a total number) and the number of matched ORB feature points(the number of ORB feature points for which it is determined that theORB feature points are the same through the matching), an error value ofan error function (a difference between feature amounts in thematching), and an estimation state (the degree of estimation accuracy ofan estimated speed and angular velocity of the mobile object 1) aregenerated as state quantities related to certainty of an estimatedposition posture in a process of executing a self-position estimationprocess of the mobile object 1.

In addition, the second self-position estimation processing unit 12sequentially outputs an estimated self-position (estimated positionposture) of the mobile object 1 and an estimation time which is the timeof the estimation, and outputs an estimated speed and angular velocity(estimated values of a translational speed and an angular velocity) ofthe mobile object 1, the number of detected ORB feature points and thenumber of matched ORB feature points, an error value of an errorfunction (a difference between feature amounts in the matching), and anestimation state (the degree of estimation accuracy of an estimatedspeed and angular velocity of the mobile object 1) as state quantitiesrelated to certainty of the estimated position posture (hereinafterreferred to as a state quantity 2). Meanwhile, a ratio of the number ofmatched ORB feature points to the number of ORB feature points (a totalnumber) may be output instead of the number of ORB feature points andthe number of matched ORB feature points.

The state quantities 1 and 2 respectively output from the first andsecond self-position estimation processing units 11 and 12 are input tothe weighting factor learning processing unit 14. In the presentembodiment, the weighting factor learning processing unit 14 isconstituted by a neural network. In addition, regarding the neuralnetwork, multimodal learning using a feature vector constituted by thestate quantities 1 and 2 as multimodal information is performed inadvance.

In more detail, the multimodal learning is learning for generating aweighting factor (weighting factors corresponding to estimated positionpostures output from the first and second self-position estimationprocessing units 11 and 12) which are equivalent to the degreeindicating to what extent the self-position estimation process of themobile object 1 using the algorithm of each of the first and secondself-position estimation processing units 11 and 12 is appropriatelyperformed as compared to other algorithms from feature vectorsconstituted by various state quantities 1 and 2.

Such multimodal learning is performed on the neural network of theweighting factor learning processing unit 14 in advance. For thisreason, the weighting factor learning processing unit 14 to which thestate quantities 1 and 2 are input from the first and secondself-position estimation processing units 11 and 12 generates weightingfactors corresponding to the first and second self-position estimationprocessing units 11 and 12 from the feature vectors constituted by thestate quantities B1 and A by the neural network and outputs thegenerated weighting factors.

In this case, the neural network basically generates and outputsweighting factors corresponding to the first and second self-positionestimation processing units 11 and 12 so that the values of theweighting factors become larger as certainty of each of the estimatedposition posture of the mobile object 1 which are output by the firstand second self-position estimation processing units 11 and 12 becomeshigher.

For example, in a situation where the self-position estimation processfor the mobile object 1 which is performed by the first self-positionestimation processing unit 11 is performed more appropriately than thatof the second self-position estimation processing unit 12, the weightingfactors are generated such that the value of the weighting factorcorresponding to the first self-position estimation processing unit 11becomes larger than the value of the weighting factor corresponding tothe second self-position estimation processing unit 12.

In addition, for example, in a situation where the self-positionestimation process for the mobile object 1 which is performed by thesecond self-position estimation processing unit 12 is performed moreappropriately than that of the first self-position estimation processingunit 11, the weighting factors are generated such that the value of theweighting factor corresponding to the second self-position estimationprocessing unit 12 becomes larger than the value of the weighting factorcorresponding to the first self-position estimation processing unit 11.

The weighting factors corresponding to the first and secondself-position estimation processing units 11 and 12 which are outputfrom the weighting factor learning processing unit 14 are input to theself-position specification processing unit 15. In addition, anestimated position posture and an estimation time of the mobile object 1are input to the self-position specification processing unit 15 fromeach of the first and second self-position estimation processing units11 and 12.

In addition, the self-position specification processing unit 15specifies a position posture obtained by combining estimated positionpostures of the first and second self-position estimation processingunits 11 and 12 at the same time with each other in accordance with theinput weighting factors as a self-position of the mobile object 1. Thatis, the sum of the estimated position postures of the first and secondself-position estimation processing units 11 and 12 multiplied by thecorresponding weighting factors is specified as a self-position of themobile object 1. Meanwhile, at this time, the sum of the weightingfactors is 1. Further, in the present embodiment, a posture in theself-position of the mobile object 1 which is specified by theself-position specification processing unit 15 is the posture of themobile object 1 in a yaw direction and does not include postures in aroll direction and a pitch direction.

In the present embodiment, the self-position specification processingunit 15 sequentially specifies self-positions of the mobile object 1 asdescribed above. In addition, the self-position specification processingunit 15 outputs a set of the specified self-position and an estimationtime corresponding thereto.

The self-position estimation processing apparatus 10 of the presentembodiment executes the process of the correction processing unit 16while executing the processes of the first and second self-positionestimation processing units 11 and 12, the weighting factor learningprocessing unit 14, and the self-position specification processing unit15 as described above. Meanwhile, in the following description, aself-position (estimated position posture) estimated by the firstself-position estimation processing unit 11 may be referred to as afirst estimated position posture, a self-position (estimated positionposture) estimated by the second self-position estimation processingunit 12 may be referred to as a second estimated position posture, and aself-position specified by the self-position specification processingunit 15 may be referred to as a specified position posture.

A set of the first estimated position posture and an estimation timewhich is output from the first self-position estimation processing unit11, a set of the second estimated position posture and an estimationtime which is output from the second self-position estimation processingunit 12, and a set of the specified position posture and an estimationtime which is output from the self-position specification processingunit 15 are input to the correction processing unit 16. In addition, thecorrection processing unit 16 stores and holds data of the input firstestimated position posture, second estimated position posture, andspecified position posture in a memory not shown in the memory in timeseries together with estimation times corresponding thereto. In thiscase, the data stored and held in the memory is data which is input tothe correction processing unit 16 in a period from the latest data tobefore a predetermined period of time.

In addition, first, the correction processing unit 16 determines whetheror not an estimation process performed by the second self-positionestimation processing unit 12 has been normally performed whenever asecond estimated position posture is input. Here, in the presentembodiment, since the mobile object 1 moves on a floor surface inside abuilding having a ceiling, the posture of the mobile object 1 does notchange much in a roll direction (a direction around an axis in afront-back direction of the mobile object 1) or a pitch direction (adirection around an axis in a right-left direction of the mobile object1). Consequently, for example, in a case where a posture in a secondestimated position posture newly determined at a time of the latestprocessing cycle by the second self-position estimation processing unit12 has changed in a roll direction or a pitch direction more than apredetermined amount from a posture in a second estimated positionposture determined at a time one processing cycle before, the correctionprocessing unit 16 determines that a self-position estimation processfor the mobile object 1 which is performed by the second self-positionestimation processing unit 12 has not been normally performed.

Further, in this case, the correction processing unit 16 instructs thesecond self-position estimation processing unit 12 to reset a firstestimated position posture output from the first self-positionestimation processing unit 11 at an estimation time corresponding to thelatest second estimated position posture or a time substantiallyconsistent with the estimation time as a new initial position posture ofthe mobile object 1 in the process of the second self-positionestimation processing unit 12, and inputs the first estimated positionposture to the first self-position estimation processing unit 12.

In this case, the second self-position estimation processing unit 12resets the input first estimated position posture as a new initialposition posture of the mobile object 1 and resets a local coordinatesystem in an estimation process performed by the second self-positionestimation processing unit 12. In addition, thereafter, the secondself-position estimation processing unit 12 executes a self-positionestimation process for the mobile object 1 using the reset localcoordinate system.

Further, in a case where changes in a posture in the second estimatedposition posture in a roll direction and a pitch direction remain belowa predetermined amount, the correction processing unit 16 determinesthat a self-position estimation process for the mobile object 1 which isperformed by the second self-position estimation processing unit 12 hasbeen normally performed.

In this case, the correction processing unit 16 executes a correctionparameter determination process to be described below, for example,whenever a predetermined period elapses (further, intermittently).Meanwhile, the predetermined period is a period, for example, from whenthe period is started to when the number of times of sampling of acaptured image of the camera 7 which is acquired by the secondself-position estimation processing unit 12 reaches a predeterminednumber.

In the correction parameter determination process, the correctionprocessing unit 16 extracts time-series data of a second estimatedposition posture which is output from the second self-positionestimation processing unit 12 and time-series data of a first estimatedposition posture which is output from the first self-position estimationprocessing unit 11 from the memory not shown in the drawing within afirst predetermined period when the first predetermined period haselapsed after the mobile object 1 starts to move. In addition,time-series data of a first estimated position posture which is outputfrom the first self-position estimation processing unit 11 andtime-series data of a specified position posture which is output fromthe self-position specification processing unit 15 are extracted fromthe memory not shown in the drawing within each predetermined periodwhen a second predetermined period or each of the subsequentpredetermined periods has elapsed. Meanwhile, regarding a timing of astarting point of the second predetermined period or each of thesubsequent predetermined periods, when an initial position posture ofthe mobile object 1 by the second self-position estimation processingunit 12 is reset as described above, a timing of the resetting is atiming of a starting point of a new predetermined period.

Here, the estimation process performed by the second self-positionestimation processing unit 12 is a process of sequentially multiplyingmovement amounts and moving directions of the mobile object 1 which aresequentially obtained from the initial position posture of the mobileobject 1. For this reason, errors of second estimated position posturesof the mobile object 1 which are sequentially obtained by the secondself-position estimation processing unit 12 may increase in anintegration manner due to an error of an initial position posture (aninitial position posture seen in a global coordinate system) of themobile object 1, estimation errors of movement amounts and movingdirections of the mobile object 1 which are sequentially obtained by thesecond self-position estimation processing unit 12, or the like.

Further, a moving track of the mobile object 1 indicated by time-seriesdata of a second estimated position posture (hereinafter, referred to asa moving track Lb) may deviate from a moving track of the mobile object1 indicated by time-series data of a first estimated position posture ortime-series data of a specified position posture (hereinafter, referredto as a moving track La) as shown in FIG. 3A. Further, in the presentembodiment, since the self-position specification processing unit 15obtains a specified position posture of the mobile object 1 as describedabove, there is a possibility that the stability of a specified positionposture determined by the self-position specification processing unit 15is poor immediately after the start of the movement of the mobile object1.

Consequently, the correction processing unit 16 determines a rotationtransformation matrix R for performing rotation transformation so thatthe moving track Lb of the mobile object 1 which is indicated bytime-series data of a second estimated position posture is brought closeto the moving track La of the mobile object 1 which is indicated bytime-series data of a first estimated position posture, using thetime-series data of the second estimated position posture which isoutput from the second self-position estimation processing unit 12 andthe time-series data of the first estimated position posture which isoutput from the first self-position estimation processing unit 11 withina first predetermined period when the first predetermined period elapsesimmediately after the start of the movement of the mobile object 1. Therotation transformation matrix R is determined using, for example, apoint cloud library (PCL) which is a known point cloud processinglibrary. A moving track Lb′ shown in FIG. 3B represents a moving trackafter the rotation transformation of the moving track Lb is performedusing the rotation transformation matrix R.

Further, the correction processing unit 16 determines a translationconverting operation amount T (for example, a translation movementvector) for performing translation movement so that a terminus of themoving track Lb′ formed by performing rotation transformation on themoving track Lb of the mobile object 1 indicated by time-series data ofa second estimated position posture using the rotation transformationmatrix R is consistent with a terminus of the moving track La of themobile object 1 indicated by time-series data of a first estimatedposition posture as shown in FIG. 3C. The translation convertingoperation amount T is obtained from the position of the terminus of themoving track Lb′ after rotation transformation (a position in a globalcoordinate system) and the position of a terminus of a moving track L2of the mobile object 1 (a position in a global coordinate system). Themoving track Lb′ shown in FIG. 3C represents a moving track aftertranslation conversion of the moving track Lb is performed using thetranslation converting operation amount T.

When each of a second predetermined period and the subsequentpredetermined periods elapses, the correction processing unit 16determines a rotation transformation matrix R and a translationconverting operation amount T as described above using time-series dataof a specified position posture which is output from the self-positionspecification processing unit 15 instead of the time-series data of thefirst estimated position posture.

In the present embodiment, the correction processing unit 16 basicallyoutputs the rotation transformation matrix R and the translationconverting operation amount T determined as described above to thesecond self-position estimation processing unit 12 as correctionparameters for correcting a second estimated position posture whenever apredetermined period elapses. However, in the present embodiment, sincethe posture of an estimated position posture obtained by the firstself-position estimation processing unit 11 and the posture of aspecified position posture specified by the self-position specificationprocessing unit 15 are postures in a yaw direction, it is preferablethat the rotation transformation matrix R rotate the second estimatedposition posture in a yaw direction or substantially a yaw direction.

Consequently, in the present embodiment, the correction processing unit16 outputs the rotation transformation matrix R and the translationconverting operation amount T to the second self-position estimationprocessing unit 12 as correction parameters only in a case where acomponent related to rotation transformation in a yaw direction in therotation transformation matrix R determined as described above(specifically, a 3×3 component) has a value sufficiently close to “1”(in detail, in a case where an absolute value of a deviation between thecomponent and “1” is smaller than a predetermined threshold value nearzero), and instructs the second self-position estimation processing unit12 to correct the second estimated position posture using the correctionparameters.

In this case, the second self-position estimation processing unit 12corrects the second estimated position posture obtained by an ORB_SLAMalgorithm as described above, using the correction parameters input fromthe correction processing unit 16 (the rotation transformation matrix Rand the translation converting operation amount T) in the followingestimation process. That is, the second self-position estimationprocessing unit 12 corrects the second estimated position posture byperforming rotation transformation on the second estimated positionposture obtained by the ORB_SLAM algorithm using the rotationtransformation matrix R and performing translation conversion using thetranslation converting operation amount T. In addition, the secondself-position estimation processing unit 12 outputs the corrected secondestimated position posture.

In addition, the correction processing unit 16 does not instruct thesecond self-position estimation processing unit 12 to correct the secondestimated position posture and does not output the rotationtransformation matrix R and the translation converting operation amountT in a case where a component related to rotation transformation in ayaw direction in the rotation transformation matrix R is not a valuesufficiently close to “1” (in a case where an absolute value of adeviation between the component and “1” is larger than a predeterminedthreshold value near zero). In this case, the second self-positionestimation processing unit 12 outputs the second estimated positionposture obtained by the ORB_SLAM algorithm without correction.

In the present embodiment, as described above, the process of theself-position estimation processing apparatus 10 is executed. In thiscase, a second estimated position posture obtained by the secondself-position estimation processing unit 12 that estimates aself-position of the mobile object 1 by an ORB_SLAM algorithm as anexample of VISUAL_SLAM is corrected in accordance with correctionparameters (a rotation transformation matrix R and a translationconverting operation amount T) determined by the correction processingunit 16 using a first estimated position posture output by the firstself-position estimation processing unit 11 or a specified positionposture output by the self-position specification processing unit 12whenever a predetermined period elapses (intermittently). Thereby, it ispossible to prevent an error of the second estimated position posturefrom increasing in an integration manner due to an error of an initialposition posture of the mobile object 1 in an estimation processperformed by the second self-position estimation processing unit 12 orerrors of movement amounts and moving directions of the mobile object 1which are sequentially obtained in the estimation process. Further,maintaining the reliability of the specified position posture output bythe self-position specification processing unit 15 can be realized withhigh robustness.

In addition, the rotation transformation matrix R and the translationconverting operation amount T as correction parameters are determinedsuch that a moving track Lb indicated by time-series data of a secondestimated position posture output by the second self-position estimationprocessing unit 12 is brought close to a moving track La indicated bytime-series data of a first estimated position posture output by thefirst self-position estimation processing unit 11 or a specifiedposition posture output by the self-position specification processingunit 15. For this reason, it is possible to determine the rotationtransformation matrix R and the translation converting operation amountT which are suitable as correction parameters.

Further, in the embodiment, self-positions respectively estimated by thefirst and second self-position estimation processing units 11 and 12 (afirst estimated position posture and a second estimated positionposture) are combined with each other using weighting factors determinedby the weighting factor learning processing unit 14 in accordance withstate quantities 1 and 2 for each of the first and second self-positionestimation processing units 11 and 12, so that a self-position of themobile object 1 is specified.

Further, in this case, since the weighting factor learning processingunit 14 uses state quantities 1 and 2 for each of the first and secondself-position estimation processing units 11 and 12 as inputinformation, weighting factors to be output in accordance with the statequantities 1 and 2 can be learned in advance in various movementenvironments of the mobile object 1.

For this reason, the self-position estimation processing apparatus 10can estimate (specify) a self-position of the mobile object 1 with highreliability in various movement environments of the mobile object 1.

Meanwhile, the disclosure is not limited to the above-describedembodiment, and other embodiments can be adopted. Hereinafter, severalother embodiments will be described.

In the embodiment, the correction processing unit 16 determinescorrection parameters (a rotation transformation matrix R and atranslation converting operation amount T) using time-series data of aspecified position posture output from the self-position specificationprocessing unit 15 when a second predetermined period or each of thesubsequent predetermined periods elapses. However, even when the secondpredetermined period or each of the subsequent predetermined periodselapses, correction parameters (a rotation transformation matrix R and atranslation converting operation amount T) may be determined usingtime-series data of a first estimated position posture output from thefirst self-position estimation processing unit 11.

Alternatively, for example, the degree of reliability or certainty of afirst estimated position posture output from the first self-positionestimation processing unit 11 may be determined on the basis of a statequantity 1 output from the first self-position estimation processingunit 11 or weighting factors determined by the weighting factor learningprocessing unit 14, and it may be selected which one out of time-seriesdata of a first estimated position posture and time-series data of aspecified position posture is to be used to determine correctionparameters in accordance with determination results. More specifically,in a case where it is determined that the reliability or certainty ofthe first estimated position posture is high, the time-series data ofthe first estimated position posture may be used to determine correctionparameters, and otherwise, the time-series data of the specifiedposition posture may be used to determine correction parameters.

Further, in the embodiment, a posture in the first estimated positionposture obtained by the first self-position estimation processing unit11 and a posture in the specified position posture obtained by theself-position specification processing unit 15 are postures only in ayaw direction, but a posture in a roll direction and a posture in apitch direction may be included.

In addition, an algorithm for an estimation process of the secondself-position estimation processing unit 12 is not limited to ORB_SLAMand may be any of other SLAM algorithms.

Further, in the embodiment, a self-position estimation processingapparatus including two self-position estimation processing units (thefirst and second self-position estimation processing units 11 and 12) asself-position estimation processing units is described. However, forexample, any of other self-position estimation processing unit capableof estimating a self-position of the mobile object 1 using any of otheralgorithms may be included instead of the first self-position estimationprocessing unit 11 or in addition to the first and second self-positionestimation processing units 11 and 12. As the other self-positionestimation processing units, for example, a self-position estimationprocessing unit that estimates a self-position of the mobile object 1using a magnetic sensor capable of being mounted in the mobile object 1,the odometry sensor 5, and a map in a magnetic state of a movementenvironment of the mobile object 1 may be adopted. In this case, as analgorithm of a self-position estimation process, an algorithm similar tothat of the first self-position estimation processing unit 11 (AMCLalgorithm) may be used.

Meanwhile, in a case where a self-position estimation processingapparatus includes three or more self-position estimation processingunits, a self-position of the mobile object 1 may be specified bydetermining weighting factors corresponding to the self-positionestimation processing units by the weighting factor learning processingunit 14 by a method similar to that in the embodiment and combiningself-positions estimated by the self-position estimation processingunits using the weighting factors with each other.

Further, in the embodiment, a neural network for performing multimodallearning is described as the weighting factor learning processing unit14. However, a recursive neural network (RNN) such as a long short-termmemory (LSTM) may be used as the weighting factor learning processingunit 14.

Further, in the embodiment, an estimated speed and angular velocity(estimated values of a translational speed and an angular velocity) ofthe mobile object 1, the number of detected ORB feature points, thenumber of matched ORB feature points, an error value (a differencebetween feature amounts in the matching) of an error function, and anestimation state are used as the state quantity 2 related to the secondself-position estimation processing unit 12. However, some statequantities among these state quantities, for example, the estimatedspeed and angular velocity of the mobile object 1, the error value ofthe error function, the estimation state, and the like may be omitted.

In addition, a method of specifying an actual self-position of themobile object 1 from self-positions estimated by a plurality ofself-position estimation processing units is not limited to the methoddescribed in the embodiment. For example, a self-position estimated byone self-position estimation processing unit selected from among theplurality of self-position estimation processing units on the basis ofan operation state of the mobile object 1 or the state of thesurrounding environment may be specified as an actual self-position ofthe mobile object 1 as it is.

Further, in the embodiment, the LRF 6 is used as a distance measurementsensor. However, for example, an ultrasonic type distance measurementsensor or the like may be used instead of the LRF 6.

Further, in the embodiment, a wheel type mobile object 1 is described asa mobile object. However, a mobile object to be applied to thedisclosure is not limited to a wheel type mobile object, and may be, forexample, a leg type mobile object, an inverted pendulum type mobileobject, or the like.

Other Configurations

According to one embodiment of the disclosure, there is provided aself-position estimation method of estimating a self-position of amobile object by each of a plurality of algorithms using detectedinformation of one or more sensors among a plurality of sensors fromdetected information of the plurality of sensors and specifying aself-position of the mobile object from estimated values of theself-positions obtained by the plurality of algorithms. The plurality ofalgorithms comprises an first algorithm which is a simultaneouslocalization and mapping (SLAM) algorithm and a second algorithm whichis an algorithm different from the SLAM, and the self-positionestimation method comprises a correction step of intermittentlycorrecting an first self-position estimated value which is an estimatedvalue of the self-position obtained by the first algorithm n accordancewith any one self-position out of a second self-position estimated valuewhich is an estimated value of the self-position obtained by the secondalgorithm and a self-position specification value which is the specifiedself-position.

Meanwhile, in the embodiment, unless otherwise specified, the“self-position” of the mobile object is a self-position seen in a globalcoordinate system which is arbitrarily set in a movement environment ofthe mobile object. In addition, the “self-position” is not limited tothe general meaning of a position of a mobile object (coordinateposition), but may include the posture (orientation) of the mobileobject.

According to the configuration, since the correction step is included,the first self-position estimated value obtained by the first algorithmwhich is an SLAM algorithm is intermittently corrected (every time apredetermined period of time elapses) in accordance with the oneself-position (the Second self-position estimated value or theself-position specification value). Thereby, it is possible to preventan error of the first self-position estimated value from increasing inan integration manner. Further, a self-position of the mobile object canbe specified using the first self-position estimated value having highreliability (a self-position specification value can be obtained).

Therefore, according to the configuration, it is possible toappropriately estimate (specify) a self-position of a mobile objectwhile appropriately correcting an estimated value of a self-position bya SLAM algorithm.

In the correction step of the configuration, parameters for correctingthe first self-position estimated value so that a track indicated by atime series of the first self-position estimated value may approach atrack indicated by a time series of the one self-position be determinedbased on the time series of the one self-position out of the secondself-position estimated value and the self-position specification valuein a predetermined period and the time series of the first self-positionestimated value in the predetermined period, and the first self-positionestimated value after the predetermined period be corrected using thedetermined parameters.

Accordingly, parameters having high reliability can be determined asparameters for correcting the first self-position estimated value.Further, it is possible to suitably correct the First self-positionestimated value by correcting the First self-position estimated valueafter the predetermined period using the determined parameters.

In the above configurations, the plurality of sensors may comprise acamera that captures an image of an outside world of the mobile object.In this case, the First algorithm may be a Visual_SLAM algorithm forsequentially executing a process of detecting a feature point from thecaptured image of the camera and a process of performing matchingbetween the detected feature point and a feature point detected from acaptured image later than the captured image.

Accordingly, since the first algorithm is a Visual_SLAM algorithm, it ispossible to suitably estimate a self-position by the first algorithm.

Further, in the above configurations, the plurality of sensors maycomprise a distance measurement sensor that measures a distance of themobile object to an external object. In this case, the second algorithmmay be an algorithm for measuring a self-position of the mobile objectusing distance measurement data obtained by the distance measurementsensor and a particulate filter.

Accordingly, in a situation where there are no other mobile objects inthe vicinity of the mobile object, or the like, a second self-positionestimated value having high reliability (in other words, high certainty)can be obtained by the second algorithm. Further, a self-positionspecification value having high reliability can be obtained on the basisof the second self-position estimated value obtained by the secondalgorithm.

For this reason, it is possible to suitably improve the reliability ofcorrection of the first self-position estimation value in accordancewith the one self-position (the second self-position estimated value orthe self-position specification value).

In the above configurations, the self-position estimation method mayfurther comprise a step of inputting a state quantity related tocertainty of the self-positions measured by each of the algorithms to alearned neural network and determining a weighting factor for each ofthe plurality of algorithms by the neural network from the input statequantity for each of the plurality of algorithms, the state quantitybeing one or more state quantities obtained through an estimationprocess of each algorithm.

A self-position obtained by combining the self-positions estimated bythe plurality of algorithms with each other using the determinedweighting factors be specified as a self-position specification value ofthe mobile object.

In a neural network for determining a weighting factor for each of theplurality of algorithms, one or more state quantities obtained throughan estimation process of each algorithm (a process of estimating aself-position of a mobile object) are input to the neural network foreach algorithm, and a weighting factor for each algorithm is determinedfrom the state quantities.

In this case, since state quantities for each algorithm are statequantities related to certainty of a self-position of a mobile objectwhich is estimated by the algorithms, the state quantities for eachalgorithm are obtained by reflecting the degree indicating to whatextent the algorithm estimation process is appropriately performed inobtaining an estimated value having high reliability of a self-positionof a mobile object.

For this reason, the neural network can determine weighting factors foreach algorithm in accordance with all state quantities to be input foreach algorithm so that weighting factors corresponding to each algorithmbecome relatively high in a situation where a self-position estimationprocess using the algorithm can be performed more appropriately thanthat using other algorithms and become relatively low in a situationwhere it is more difficult to appropriately perform a self-positionestimation process using the algorithm than to perform a self-positionestimation process using other algorithms through preliminary learningof the neural network.

In addition, weighting factors for each algorithm which are determinedby the neural network are determined depending on the state quantitiesfor each algorithm instead of the accuracy of a self-position of amobile object which is estimated by the algorithms, and thus theweighting factors are hardly affected by a place in a movementenvironment of the mobile object.

In addition, according to the configuration, a position obtained bycombining the self-positions estimated by the plurality of algorithmswith each other using weighting factors determined as described above isspecified as a self-position specification value of the mobile object.

Thereby, according to the configuration, it is possible to estimate aself-position of a mobile object in a state where it is difficult to beaffected by a movement environment of a mobile object.

As a supplement, in the above configuration, in a case where the firstalgorithm is an algorithm (Visual_SLAM algorithm), the state quantitiesaccording to the first algorithm comprise, for example, a state quantityindicating a ratio of the number of feature points associated by thematching to the total number of feature points detected from thecaptured image. Thereby, the state quantity is suitable as a statequantity related to certainty of a self-position of the mobile objectwhich is estimated by the first algorithm.

Further, in the configuration, in a case where the second algorithm isan algorithm, the state quantity according to the second algorithminclude a covariance generated in a process in which a self-position ofthe mobile object is estimated by the second algorithm. Thereby, thestate quantity is suitable as a state quantity related to certainty of aself-position of a mobile object which is estimated by the secondalgorithm.

It will be apparent to those skilled in the art that variousmodifications and variations can be made to the disclosed embodimentswithout departing from the scope or spirit of the disclosure. In view ofthe foregoing, it is intended that the disclosure covers modificationsand variations provided that they fall within the scope of the followingclaims and their equivalents.

What is claimed is:
 1. A self-position estimation method, comprising:estimating a first self-position of a mobile object by a first algorithmusing detected information of a first sensor, wherein a firstself-position estimated value is an estimated value of the firstself-position obtained by the first algorithm; estimating a secondself-position of the mobile object by a second algorithm using detectedinformation of second sensor to generate a second self-positionestimated value, wherein a second self-position estimated value is anestimated value of the second self-position obtained by the secondalgorithm; specifying a self-position of the mobile object from thefirst estimated value of the first self-position obtained by the firstalgorithm and the second estimated value of the second self-positionobtained by the second algorithm; generating correction parameters forcorrecting the first self-position estimated value based on the firstself-position, the second self-position or a self-position specificationvalue that is the specified self-position, wherein the parametersincludes a rotation transformation matrix and a translation convertingoperation amount; and intermittently correcting the first self-positionestimated value with the correction parameters, wherein the rotationtransformation matrix is determined by using time-series data of thefirst self-position estimated value and time-series data of the secondself-position estimated value in a manner that a first moving track ofthe mobile object indicated by time-series data of the secondself-position estimated value is brought close to a second moving trackof the mobile object indicated by time-series data of the firstself-position estimated value, the translation converting operationamount is determined in a manner that an end of the second moving trackis consistent with an end of a rotated moving track that is obtained byperforming a rotation transformation on the first moving track using therotation transformation matrix, and wherein the plurality of algorithmscomprises a first algorithm that is a simultaneous localization andmapping (SLAM) algorithm and a second algorithm that is an algorithmdifferent from the SLAM algorithm.
 2. The self-position estimationmethod according to claim 1, wherein the plurality of sensors comprisesa camera that captures an image of an outside world of the mobileobject, and the first algorithm is for sequentially executing a processof detecting a feature point from a captured image of the camera and aprocess of performing matching between the detected feature point and afeature point detected from a captured image later than the capturedimage.
 3. The self-position estimation method according to claim 1,wherein the plurality of sensors comprises a distance measurement sensorthat measures a distance of the mobile object to an external object, andthe second algorithm is an algorithm for estimating the secondself-position of the mobile object using distance measurement dataobtained by the distance measurement sensor and a particulate filter. 4.The self-position estimation method according to claim 2, wherein theplurality of sensors comprises a distance measurement sensor thatmeasures a distance of the mobile object to an external object, and thesecond algorithm is an algorithm for estimating the second self-positionof the mobile object using distance measurement data obtained by thedistance measurement sensor and a particulate filter.
 5. Theself-position estimation method according to claim 1, furthercomprising: a step of inputting a state quantity related to certainty ofthe first and the second self-positions respectively estimated by thefirst and the second algorithms to a learned neural network anddetermining a weighting factor for each of the first and the secondalgorithms by the neural network from the input state quantity for thefirst and the second algorithms, the state quantity being one or morestate quantities obtained through an estimation process of the first andthe second algorithms, wherein the self-position is obtained bycombining the first and the second self-positions respectively estimatedby the first and the second algorithms with each other using thedetermined weighting factors and is specified as the self-positionspecification value of the mobile object.
 6. The self-positionestimation method according to claim 2, further comprising: a step ofinputting a state quantity related to certainty of the first and thesecond self-positions respectively estimated by the first and the secondalgorithms to a learned neural network and determining a weightingfactor for each of the first and the second algorithms by the neuralnetwork from the input state quantity for the first and the secondalgorithms, the state quantity being one or more state quantitiesobtained through an estimation process of the first and the secondalgorithms, wherein a self-position is obtained by combining the firstand the second self-positions respectively estimated by the first andthe second algorithms with each other using the determined weightingfactors and is specified as the self-position specification value of themobile object.
 7. The self-position estimation method according to claim3, further comprising: a step of inputting a state quantity related tocertainty of the first and the second self-positions respectivelyestimated by the first and the second algorithms to a learned neuralnetwork and determining a weighting factor for each of the first and thesecond algorithms by the neural network from the input state quantityfor the first and the second algorithms, the state quantity being one ormore state quantities obtained through an estimation process of thefirst and the second algorithms, wherein the self-position is obtainedby combining the first and the second self-positions respectivelyestimated by the first and the second algorithms with each other usingthe determined weighting factors and is specified as the self-positionspecification value of the mobile object.